
#include "sysclk.h"
#include "uotghs_otg.h"
#include "uotghs_host.h"
#include <string.h>
#include <stdlib.h>


#ifndef UHD_USB_INT_LEVEL
# define UHD_USB_INT_LEVEL 0 //By default USB interrupt have low priority
#endif

#define USB_HOST_MAX_EP  0 
#define UHD_PIPE_USED(pipe)      (USB_HOST_MAX_EP >= pipe)

#if (     (UHD_PIPE_USED( 1) && Is_uhd_pipe_dma_supported( 1)) \
	||(UHD_PIPE_USED( 2) && Is_uhd_pipe_dma_supported( 2)) \
	||(UHD_PIPE_USED( 3) && Is_uhd_pipe_dma_supported( 3)) \
	||(UHD_PIPE_USED( 4) && Is_uhd_pipe_dma_supported( 4)) \
	||(UHD_PIPE_USED( 5) && Is_uhd_pipe_dma_supported( 5)) \
	||(UHD_PIPE_USED( 6) && Is_uhd_pipe_dma_supported( 6)) \
	||(UHD_PIPE_USED( 7) && Is_uhd_pipe_dma_supported( 7)) \
	||(UHD_PIPE_USED( 8) && Is_uhd_pipe_dma_supported( 8)) \
	||(UHD_PIPE_USED( 9) && Is_uhd_pipe_dma_supported( 9)) \
	||(UHD_PIPE_USED(10) && Is_uhd_pipe_dma_supported(10)) \
	||(UHD_PIPE_USED(11) && Is_uhd_pipe_dma_supported(11)) \
	||(UHD_PIPE_USED(12) && Is_uhd_pipe_dma_supported(12)) \
	||(UHD_PIPE_USED(13) && Is_uhd_pipe_dma_supported(13)) \
	||(UHD_PIPE_USED(14) && Is_uhd_pipe_dma_supported(14)) \
	||(UHD_PIPE_USED(15) && Is_uhd_pipe_dma_supported(15)) \
	)
# define UHD_PIPE_DMA_SUPPORTED
#endif

#if (     (UHD_PIPE_USED( 1) && !Is_uhd_pipe_dma_supported( 1)) \
	||(UHD_PIPE_USED( 2) && !Is_uhd_pipe_dma_supported( 2)) \
	||(UHD_PIPE_USED( 3) && !Is_uhd_pipe_dma_supported( 3)) \
	||(UHD_PIPE_USED( 4) && !Is_uhd_pipe_dma_supported( 4)) \
	||(UHD_PIPE_USED( 5) && !Is_uhd_pipe_dma_supported( 5)) \
	||(UHD_PIPE_USED( 6) && !Is_uhd_pipe_dma_supported( 6)) \
	||(UHD_PIPE_USED( 7) && !Is_uhd_pipe_dma_supported( 7)) \
	||(UHD_PIPE_USED( 8) && !Is_uhd_pipe_dma_supported( 8)) \
	||(UHD_PIPE_USED( 9) && !Is_uhd_pipe_dma_supported( 9)) \
	||(UHD_PIPE_USED(10) && !Is_uhd_pipe_dma_supported(10)) \
	||(UHD_PIPE_USED(11) && !Is_uhd_pipe_dma_supported(11)) \
	||(UHD_PIPE_USED(12) && !Is_uhd_pipe_dma_supported(12)) \
	||(UHD_PIPE_USED(13) && !Is_uhd_pipe_dma_supported(13)) \
	||(UHD_PIPE_USED(14) && !Is_uhd_pipe_dma_supported(14)) \
	||(UHD_PIPE_USED(15) && !Is_uhd_pipe_dma_supported(15)) \
	)
# define UHD_PIPE_FIFO_SUPPORTED
#endif

#ifndef UHD_ISOCHRONOUS_NB_BANK
#  define UHD_ISOCHRONOUS_NB_BANK 2
#else
#  if (UHD_ISOCHRONOUS_NB_BANK < 1) || (UHD_ISOCHRONOUS_NB_BANK > 2)
#    error UHD_ISOCHRONOUS_NB_BANK must be define with 1 or 2.
#  endif
#endif

#ifndef UHD_BULK_NB_BANK
#  define UHD_BULK_NB_BANK        2
#else
#  if (UHD_BULK_NB_BANK < 1) || (UHD_BULK_NB_BANK > 2)
#    error UHD_BULK_NB_BANK must be define with 1 or 2.
#  endif
#endif

#ifndef UHD_INTERRUPT_NB_BANK
#  define UHD_INTERRUPT_NB_BANK   1
#else
#  if (UHD_INTERRUPT_NB_BANK < 1) || (UHD_INTERRUPT_NB_BANK > 2)
#    error UHD_INTERRUPT_NB_BANK must be define with 1 or 2.
#  endif
#endif

bool uhd_b_suspend_requested;
uint8_t uhc_sof_timeout;
volatile uint16_t uhd_ctrl_request_timeout = 0;
uhd_ctrl_request_phase_t uhd_ctrl_request_phase;
struct uhd_ctrl_request_t *uhd_ctrl_request_first = NULL;
struct uhd_ctrl_request_t *uhd_ctrl_request_last = NULL;
uint16_t uhd_ctrl_nb_trans;
uhc_sof_timeout_callback_t uhc_sof_timeout_callback;


//--------------------------------------------------------
//--- INTERNAL ROUTINES TO MANAGED GLOBAL EVENTS

void uhd_enable(void)
{
	irqflags_t flags;
	uhc_sof_timeout = 0; 

	// To avoid USB interrupt before end of initialization
	flags = cpu_irq_save();

	sysclk_enable_usb();
	pmc_enable_periph_clk(ID_UOTGHS);

	//set interrupt vector, %by Santiago
	NVIC_SetPriority((IRQn_Type) ID_UOTGHS, 1);
	NVIC_EnableIRQ((IRQn_Type) ID_UOTGHS);
	PMC->PMC_FSMR |= (PMC_FSMR_USBAL & PMC_FAST_STARTUP_Msk);

	//force host mode, %by Santiago
	Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_UIDE);        
	Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_UIMOD);       

	// not to use otg PAD
	Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_OTGPADE);

	//this is not really enable otg function, but enable usb, %States by Santiago
	Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_USBE);
	//Wr_bits(UOTGHS->UOTGHS_HSTCTRL, UOTGHS_HSTCTRL_SPDCONF_Msk, UOTGHS_HSTCTRL_SPDCONF_FORCED_FS);
	Wr_bits(UOTGHS->UOTGHS_HSTCTRL, UOTGHS_HSTCTRL_SPDCONF_Msk, UOTGHS_HSTCTRL_SPDCONF_HIGH_SPEED);
	
	//UOTGHS->UOTGHS_HSTCTRL |= (0x02 << 12);

	// Check USB clock
	Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_FRZCLK);
	while (!Is_otg_clock_usable());

	// Clear all interrupts that may have been set by a previous host mode
	UOTGHS->UOTGHS_HSTICR = UOTGHS_HSTICR_DCONNIC | UOTGHS_HSTICR_DDISCIC
		| UOTGHS_HSTICR_HSOFIC  | UOTGHS_HSTICR_HWUPIC
		| UOTGHS_HSTICR_RSMEDIC | UOTGHS_HSTICR_RSTIC
		| UOTGHS_HSTICR_RXRSMIC;

	UOTGHS->UOTGHS_SCR = UOTGHS_SCR_VBUSTIC;

	// Enable Vbus change and error interrupts
	// Disable automatic Vbus control after Vbus error
	//not to use VBOF pin, so need not to use VBUS interrupts, %By Santiago
	Set_bits(UOTGHS->UOTGHS_CTRL,
		UOTGHS_CTRL_VBUSHWC | UOTGHS_CTRL_VBUSTE);// | UOTGHS_CTRL_VBERRE);

	UOTGHS->UOTGHS_SFR = UOTGHS_SR_VBUSRQ;

	// Force Vbus interrupt when Vbus is always high
	// This is possible due to a short timing between a Host mode stop/start.
	if (Is_otg_vbus_high()) 
	{
		UOTGHS->UOTGHS_SFR = UOTGHS_SFR_VBUSTIS;
	}

	// Enable main control interrupt
	// Connection, SOF and reset
	UOTGHS->UOTGHS_HSTIER = UOTGHS_HSTICR_DCONNIC | UOTGHS_HSTICR_HSOFIC
		| UOTGHS_HSTICR_RSTIC;

	Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_FRZCLK);

	cpu_irq_restore(flags);
}

void uhd_disable(bool b_id_stop)
{
	UNUSED(b_id_stop);
	irqflags_t flags;

	flags = cpu_irq_save();  
	// Check USB clock ready after a potential sleep mode < IDLE
	while (!Is_otg_clock_usable());
	Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_FRZCLK);

	// Disable Vbus change and error interrupts
	Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_VBUSTE | UOTGHS_CTRL_VBERRE);

	// Disable main control interrupt
	// (Connection, disconnection, SOF and reset)
	UOTGHS->UOTGHS_HSTIDR = UOTGHS_HSTIDR_DCONNIEC | UOTGHS_HSTIDR_DDISCIEC
		| UOTGHS_HSTIDR_RSTIEC   | UOTGHS_HSTIDR_RSMEDIEC
		| UOTGHS_HSTIDR_RXRSMIEC | UOTGHS_HSTIDR_HSOFIEC
		| UOTGHS_HSTIDR_HWUPIEC;

	Clr_bits(UOTGHS->UOTGHS_HSTCTRL, UOTGHS_HSTCTRL_SOFE);                  

	// Unalloc pipe
	uhd_disable_pipe(0);
	uhd_unallocate_memory(0);

	// Stop transfer on this pipe
	// Endpoint interrupt, bulk or isochronous
	Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_FRZCLK);

	otg_disable();
	otg_disable_pad();
	sysclk_disable_usb();
	pmc_disable_periph_clk(ID_UOTGHS);

	// Do not authorize asynchronous USB interrupts
	/*PMC->PMC_FSMR &= ~(PMC_FSMR_USBAL & PMC_FAST_STARTUP_Msk);
	Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_USBE);
	PMC->CKGR_UCKR &= ~CKGR_UCKR_UPLLEN;

	if ((PMC->PMC_PCSR1 & (1u << (ID_UOTGHS - 32))) == (1u << (ID_UOTGHS - 32))) 
	{
	PMC->PMC_PCDR1 = 1 << (ID_UOTGHS - 32);
	}*/
	cpu_irq_restore(flags);
}

uhd_speed_t uhd_get_speed(void)
{
	switch (uhd_get_speed_mode()) {

	case UOTGHS_SR_SPEED_HIGH_SPEED:
		return UHD_SPEED_HIGH;

	case UOTGHS_SR_SPEED_FULL_SPEED:
		return UHD_SPEED_FULL;

	case UOTGHS_SR_SPEED_LOW_SPEED:
		return UHD_SPEED_LOW;

	default:
		return UHD_SPEED_LOW;
	}
}
#ifdef SUPPORT_ACCESSORY
bool uhd_ep_alloc(usb_add_t add, uint8_t pipeNum, uint8_t epNum, uint8_t ep_size, Bool isIn)
{
	uhd_enable_pipe(pipeNum);
	uhd_configure_pipe(pipeNum, // Pipe Num
		0, // No frequency
		epNum, // endpoint Num
		UOTGHS_HSTPIPCFG_PTYPE_BLK,
		isIn ? UOTGHS_HSTPIPCFG_PTOKEN_IN : UOTGHS_HSTPIPCFG_PTOKEN_OUT,
		ep_size,
		UOTGHS_HSTPIPCFG_PBK_1_BANK, 1);
	
	uhd_allocate_memory(pipeNum);
	if (!Is_uhd_pipe_configured(pipeNum)) {
		uhd_disable_pipe(pipeNum);
		return false;
	}
	uhd_configure_address(pipeNum, add);

	// Always enable stall and error interrupts of control endpoint
	uhd_enable_stall_interrupt(pipeNum);
	uhd_enable_pipe_error_interrupt(pipeNum);
	uhd_enable_pipe_interrupt(pipeNum);
	return true;
}
#endif
bool uhd_ep0_alloc(usb_add_t add, uint8_t ep_size)
{
	if (ep_size < 8) {
		return false;
	}

	uhd_enable_pipe(0);
	uhd_configure_pipe(0, // Pipe 0
		0, // No frequency
		0, // endpoint 0
		UOTGHS_HSTPIPCFG_PTYPE_CTRL,
		UOTGHS_HSTPIPCFG_PTOKEN_SETUP,

		ep_size,

		UOTGHS_HSTPIPCFG_PBK_1_BANK, 0);

	uhd_allocate_memory(0);
	if (!Is_uhd_pipe_configured(0)) {
		uhd_disable_pipe(0);
		return false;
	}
	uhd_configure_address(0, add);

	// Always enable stall and error interrupts of control endpoint
	uhd_enable_stall_interrupt(0);
	uhd_enable_pipe_error_interrupt(0);
	uhd_enable_pipe_interrupt(0);
	return true;
}

void uhd_ep_free(usb_add_t add, usb_ep_t endp)
{
	// Search endpoint(s) in all pipes
	for (uint8_t pipe = 0; pipe < UOTGHS_EPT_NUM; pipe++) {
		if (!Is_uhd_pipe_enabled(pipe)) {
			continue;
		}
		if (add != uhd_get_configured_address(pipe)) {
			continue;
		}
		if (endp != 0xFF) {
			// Disable specific endpoint number
			if (endp != uhd_get_pipe_endpoint_address(pipe)) {
				continue; // Mismatch
			}
		}
		// Unalloc pipe
		uhd_disable_pipe_interrupt(pipe);//by Santiago
		uhd_disable_pipe(pipe);
		uhd_unallocate_memory(pipe);
	}
}

void uhd_ctrl_timeout(void)
{
	if (uhd_ctrl_request_timeout) 
	{
		// Setup request on-going
		if (--uhd_ctrl_request_timeout == 0) {
			// Stop request
			uhd_freeze_pipe(0);
			uhd_ctrl_request_end(UHD_TRANS_TIMEOUT);
		}
	}
}

void uhd_ctrl_phase_setup(void)
{
	union{
		volatile uint64_t value64;
		usb_setup_req_t req;
	} setup;
	volatile uint64_t *ptr_ep_data;

	uhd_ctrl_request_phase = UHD_CTRL_REQ_PHASE_SETUP;
	memcpy(&setup.req, &uhd_ctrl_request_first->req, sizeof(usb_setup_req_t));

	// Manage LSB/MSB to fit with CPU usage
	setup.req.wValue = cpu_to_le16(setup.req.wValue);
	setup.req.wIndex = cpu_to_le16(setup.req.wIndex);
	setup.req.wLength = cpu_to_le16(setup.req.wLength);
	uhd_ctrl_nb_trans = 0;

	// Check pipe
	if (!Is_uhd_pipe_enabled(0) ||
		(uhd_ctrl_request_first->add != uhd_get_configured_address(0))) {
			uhd_ctrl_request_end(UHD_TRANS_DISCONNECT);
			return; // Endpoint not valid
	}

	// Fill pipe
	uhd_configure_pipe_token(0, UOTGHS_HSTPIPCFG_PTOKEN_SETUP);
	uhd_ack_setup_ready(0);

	ptr_ep_data = (volatile uint64_t *)&uhd_get_pipe_fifo_access(0, 64);
	*ptr_ep_data = setup.value64;

	uhd_ctrl_request_timeout = 5000;
	uhd_enable_setup_ready_interrupt(0);
	uhd_ack_fifocon(0);
	uhd_unfreeze_pipe(0);
}

bool uhd_setup_request(
	usb_add_t add,
	usb_setup_req_t *req,
	uint8_t *payload,
	uint16_t payload_size,
	uhd_callback_setup_run_t callback_run,
	uhd_callback_setup_end_t callback_end)
{
	irqflags_t flags;
	struct uhd_ctrl_request_t *request;
	bool b_start_request = false;

	request = malloc(sizeof(struct uhd_ctrl_request_t));
	if (request == NULL) 
	{
		return false;
	}

	// Fill structure
	request->add = (uint8_t) add;
	memcpy(&request->req, req, sizeof(usb_setup_req_t));
	request->payload = payload;
	request->payload_size = payload_size;
	request->callback_run = callback_run;
	request->callback_end = callback_end;
	request->next_request = NULL;

	// Add this request in the queue
	flags = cpu_irq_save();
	if (uhd_ctrl_request_first == NULL) {
		uhd_ctrl_request_first = request;
		b_start_request = true;
	} else {
		uhd_ctrl_request_last->next_request = request;
	}
	uhd_ctrl_request_last = request;
	cpu_irq_restore(flags);

	if (b_start_request) 
	{
		// Start immediately request
		uhd_ctrl_phase_setup();
	}
	return true;
}

void uhd_ctrl_request_end(uhd_trans_status_t status)
{
	irqflags_t flags;
	uhd_callback_setup_end_t callback_end;
	struct uhd_ctrl_request_t *request_to_free;
	bool b_new_request;

	uhd_ctrl_request_timeout = 0;

	// Remove request from the control request list
	callback_end = uhd_ctrl_request_first->callback_end;
	request_to_free = uhd_ctrl_request_first;
	flags = cpu_irq_save();
	uhd_ctrl_request_first = uhd_ctrl_request_first->next_request;
	b_new_request = (uhd_ctrl_request_first != NULL);
	cpu_irq_restore(flags);
	free(request_to_free);

	// Call callback
	if (callback_end != NULL) {
		callback_end(uhd_get_configured_address(0), status, uhd_ctrl_nb_trans);
	}

	// If a setup request is pending and no started by previous callback
	if (b_new_request) {
		uhd_ctrl_phase_setup();
	}
	if (uhd_b_suspend_requested) {
		// A suspend request has been delay after all setup request
		uhd_b_suspend_requested = false;
		//uhd_suspend();
	}
}

void uhd_sof_interrupt(void)
{
	//Clear SOR interrupt flag
	UOTGHS->UOTGHS_HSTICR = UOTGHS_HSTICR_HSOFIC;//uhd_ack_sof();

	// Manage the micro SOF
	if (Is_uhd_high_speed_mode()) 
	{
		static uint8_t msof_cpt = 0;
		if (++msof_cpt % 8) {
			// It is a micro SOF  
			// then send Micro start of frame event (each 125us)
			//uhc_notify_sof(true); //has not any micro sof process, %by Santiago

			return;
		}
	}

	// Manage the timeout on endpoint control transfer
	uhd_ctrl_timeout();

	// Manage the timeouts on endpoint transfer
	if (uhc_sof_timeout) 
	{
		if (--uhc_sof_timeout == 0) 
		{
			uhc_sof_timeout_callback();
		}
	}
}

void uhd_ctrl_interrupt(void)
{
	// Disable setup, IN and OUT interrupts of control endpoint
	UOTGHS->UOTGHS_HSTPIPIDR[0] = UOTGHS_HSTPIPIDR_TXSTPEC
		| UOTGHS_HSTPIPIDR_RXINEC
		| UOTGHS_HSTPIPIDR_TXOUTEC;

	// Search event on control endpoint
	if (Is_uhd_setup_ready(0)) {
		// SETUP packet sent
		UOTGHS->UOTGHS_HSTPIPIER[0] = UOTGHS_HSTPIPIER_PFREEZES;
		UOTGHS->UOTGHS_HSTPIPICR[0] = UOTGHS_HSTPIPICR_TXSTPIC;

		// Start DATA phase
		if ((uhd_ctrl_request_first->req.bmRequestType & USB_REQ_DIR_MASK) == USB_REQ_DIR_IN) 
		{
			uhd_ctrl_phase_data_in_start();
		} 
		else
		{
			if (uhd_ctrl_request_first->req.wLength) 
			{
				uhd_ctrl_phase_data_out();
			} 
			else 
			{
				// No DATA phase
				uhd_ctrl_phase_zlp_in();
			}
		}
		return;
	}
	if (Is_uhd_in_received(0)) {
		// In case of low USB speed and with a high CPU frequency,
		// a ACK from host can be always running on USB line
		// then wait end of ACK on IN pipe.
		while (!Is_uhd_pipe_frozen(0));

		// IN packet received
		uhd_ack_in_received(0);
		switch(uhd_ctrl_request_phase) {
		case UHD_CTRL_REQ_PHASE_DATA_IN:
			uhd_ctrl_phase_data_in();
			break;
		case UHD_CTRL_REQ_PHASE_ZLP_IN:
			uhd_ctrl_request_end(UHD_TRANS_NOERROR);
			break;
		default:
			break;
		}
		return;
	}
	if (Is_uhd_out_ready(0)) {
		// OUT packet sent
		uhd_freeze_pipe(0);
		uhd_ack_out_ready(0);
		switch(uhd_ctrl_request_phase) {
		case UHD_CTRL_REQ_PHASE_DATA_OUT:
			uhd_ctrl_phase_data_out();
			break;
		case UHD_CTRL_REQ_PHASE_ZLP_OUT:
			uhd_ctrl_request_end(UHD_TRANS_NOERROR);
			break;
		default:
			break;
		}
		return;
	}
	if (Is_uhd_stall(0)) {
		// Stall Handshake received
		uhd_ack_stall(0);
		uhd_ctrl_request_end(UHD_TRANS_STALL);
		return;
	}
	if (Is_uhd_pipe_error(0)) {
		// Get and ack error
		uhd_ctrl_request_end(uhd_pipe_get_error(0));
		return;
	}
}

void uhd_ctrl_phase_data_in_start(void)
{
	uhd_ctrl_request_phase = UHD_CTRL_REQ_PHASE_DATA_IN;
	uhd_configure_pipe_token(0, UOTGHS_HSTPIPCFG_PTOKEN_IN);
	uhd_ack_in_received(0);
	uhd_ack_short_packet(0);
	uhd_enable_in_received_interrupt(0);
	uhd_ack_fifocon(0);
	uhd_unfreeze_pipe(0);
}

void uhd_ctrl_phase_data_in(void)
{
	bool b_short_packet;
	uint8_t *ptr_ep_data;
	uint8_t nb_byte_received;

	// Get information to read data
	nb_byte_received = uhd_byte_count(0);
#ifdef USB_HOST_HUB_SUPPORT
	//! In HUB mode, the control pipe is always configured to 64B
	//! thus the short packet flag must be computed
	b_short_packet = (nb_byte_received != uhd_get_pipe_size(0));
	uhd_ack_short_packet(0);
#else
	b_short_packet = Is_uhd_short_packet(0);
#endif

	ptr_ep_data = (uint8_t *) & uhd_get_pipe_fifo_access(0, 8);
uhd_ctrl_receiv_in_read_data:
	// Copy data from pipe to payload buffer
	while (uhd_ctrl_request_first->payload_size && nb_byte_received) {
		*uhd_ctrl_request_first->payload++ = *ptr_ep_data++;
		uhd_ctrl_nb_trans++;
		uhd_ctrl_request_first->payload_size--;
		nb_byte_received--;
	}

	if (!uhd_ctrl_request_first->payload_size && nb_byte_received) {
		// payload buffer is full to store data remaining
		if (uhd_ctrl_request_first->callback_run == NULL
			|| !uhd_ctrl_request_first->callback_run(
			uhd_get_configured_address(0),
			&uhd_ctrl_request_first->payload,
			&uhd_ctrl_request_first->payload_size)) {
				// DATA phase aborted by host
				goto uhd_ctrl_phase_data_in_end;
		}
		// The payload buffer has been updated by the callback
		// thus the data load can restart.
		goto uhd_ctrl_receiv_in_read_data;
	}

	// Test short packet
	if ((uhd_ctrl_nb_trans == uhd_ctrl_request_first->req.wLength)
		|| b_short_packet) {
			// End of DATA phase or DATA phase abort from device
uhd_ctrl_phase_data_in_end:
			uhd_ctrl_phase_zlp_out();
			return;
	}

	// Send a new IN packet request
	uhd_enable_in_received_interrupt(0);
	uhd_ack_fifocon(0);
	uhd_unfreeze_pipe(0);
}

void uhd_ctrl_phase_zlp_in(void)
{
	uhd_ctrl_request_phase = UHD_CTRL_REQ_PHASE_ZLP_IN;
	uhd_configure_pipe_token(0, UOTGHS_HSTPIPCFG_PTOKEN_IN);
	uhd_ack_in_received(0);
	uhd_ack_short_packet(0);
	uhd_enable_in_received_interrupt(0);
	uhd_ack_fifocon(0);
	uhd_unfreeze_pipe(0);
}

void uhd_ctrl_phase_data_out(void)
{
	uint8_t *ptr_ep_data;
	uint8_t ep_ctrl_size;

	uhd_ctrl_request_phase = UHD_CTRL_REQ_PHASE_DATA_OUT;

	if (uhd_ctrl_nb_trans == uhd_ctrl_request_first->req.wLength) {
		// End of DATA phase
		uhd_ctrl_phase_zlp_in();
		return;
	}

	if (!uhd_ctrl_request_first->payload_size) {
		// Buffer empty, then request a new buffer
		if (uhd_ctrl_request_first->callback_run==NULL
			|| !uhd_ctrl_request_first->callback_run(
			uhd_get_configured_address(0),
			&uhd_ctrl_request_first->payload,
			&uhd_ctrl_request_first->payload_size)) {
				// DATA phase aborted by host
				uhd_ctrl_phase_zlp_in();
				return;
		}
	}

#ifdef USB_HOST_HUB_SUPPORT
	// TODO
#else
	ep_ctrl_size = uhd_get_pipe_size(0);
#endif

	// Fill pipe
	uhd_configure_pipe_token(0, UOTGHS_HSTPIPCFG_PTOKEN_OUT);
	uhd_ack_out_ready(0);
	ptr_ep_data = (uint8_t *) & uhd_get_pipe_fifo_access(0, 8);
	while ((uhd_ctrl_nb_trans < uhd_ctrl_request_first->req.wLength)
		&& ep_ctrl_size && uhd_ctrl_request_first->payload_size) {
			*ptr_ep_data++ = *uhd_ctrl_request_first->payload++;
			uhd_ctrl_nb_trans++;
			ep_ctrl_size--;
			uhd_ctrl_request_first->payload_size--;
	}
	uhd_enable_out_ready_interrupt(0);
	uhd_ack_fifocon(0);
	uhd_unfreeze_pipe(0);
}

void uhd_ctrl_phase_zlp_out(void)
{
	uhd_ctrl_request_phase = UHD_CTRL_REQ_PHASE_ZLP_OUT;
	uhd_configure_pipe_token(0, UOTGHS_HSTPIPCFG_PTOKEN_OUT);
	uhd_ack_out_ready(0);
	uhd_enable_out_ready_interrupt(0);
	uhd_ack_fifocon(0);
	uhd_unfreeze_pipe(0);
}

uhd_trans_status_t uhd_pipe_get_error(uint8_t pipe)
{
	uint32_t error = uhd_error_status(pipe) &
		(UOTGHS_HSTPIPERR_DATATGL |
		UOTGHS_HSTPIPERR_TIMEOUT |
		UOTGHS_HSTPIPERR_PID |
		UOTGHS_HSTPIPERR_DATAPID);
	uhd_ack_all_errors(pipe);
	switch(error) {
	case UOTGHS_HSTPIPERR_DATATGL:
		return UHD_TRANS_DT_MISMATCH;
	case UOTGHS_HSTPIPERR_TIMEOUT:
		return UHD_TRANS_NOTRESPONDING;
	case UOTGHS_HSTPIPERR_DATAPID:
	case UOTGHS_HSTPIPERR_PID:
	default:
		return UHD_TRANS_PIDFAILURE;
	}
}
